/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.techhighteam675.robot2012;

import edu.wpi.first.wpilibj.Joystick;

/**
 *
 * @author TJ
 */
public class TankDrive implements DriveType {
    private Joystick leftStick, rightStick;
    private boolean squareInputs;
    private int shiftSetting;

    public TankDrive(boolean squareInputs) {
        leftStick = new Joystick(Definitions.DRIVE_STICK_1);
        rightStick = new Joystick(Definitions.DRIVE_STICK_2);
        this.squareInputs = squareInputs;
        shiftSetting = DriveType.SHIFT_LOW;
    }

    public boolean isPIDControl() {
        return false;
    }

    public double getLeftSpeed() {
        float value = (float) leftStick.getY();
        System.out.println(leftStick.getY());
        return value;
        //return -(squareInputs ? value : value * value * (value / Math.abs(value)));
    }

    public double getRightSpeed() {
        float value = (float) rightStick.getY();
        return value;
        //return -(squareInputs ? value : value * value * (value / Math.abs(value)));
    }

    public int getShiftSetting() {
        if(rightStick.getRawButton(4))
            shiftSetting = DriveType.SHIFT_LOW;

        if(rightStick.getRawButton(5))
            shiftSetting = DriveType.SHIFT_HIGH;

        return shiftSetting;
    }

    public boolean isPickup() {
        return rightStick.getRawButton(1);
    }

    public int getRampArm() {
        if(rightStick.getRawButton(3))
            return 1;
        else if(rightStick.getRawButton(2))
            return -1;
        else
            return 0;
    }
}
